#include "common_math.h"


/**
*@brief     symbolic function
*
*@param     pNumber original value
*
*@return    float 0 or 1 or -1
*/
float Sgn(float fpNumber)
{
	if(fpNumber > 0)
	{
		return 1;
	}
	else if(fpNumber < 0)
	{
		return -1;
	}
	else
	{
		return 0;
	}
}



/**
*@brief clipping function, remove the value beyond the maximum and minimum value, and replace it with the maximum or minimum value
*
*@param value: input data
*@param min: clipping minimum value
*@param max: clipping maximum
*
*@return clipped data
*/
float Clip(float value, float min, float max)
{
	float ret;
	if(value < min)
	{
		ret = min;
	}
	else if(value > max)
	{
		ret = max;
	}
	else
	{
		ret = value;
	}
	return ret;
}



/**
*@brief Fast inverse square-root
* 
*@param 
*/
float invSqrt(float x)
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

/**
 * @brief safe atan2, deal with x == 0 or y == 0
 *
 * @param y
 * @param x
 * @return arctan(y/x)
 */
float safeAtan2(float y, float x)
{
	if (fabs(x) <= 1e-6 && fabs(y) <= 1e-6)
	{
		return 0.0 / RADIAN2DEGREE_VALUE;
	}
	else if (fabs(x) <= 1e-6 && y > 1e-6)
	{
		return 90.0 / RADIAN2DEGREE_VALUE;
	}
	else if (fabs(x) <= 1e-6 && y < -1e-6)
	{
		return -90.0 / RADIAN2DEGREE_VALUE;
	}
	else if (x > 1e-6 && fabs(y) <= 1e-6)
	{
		return 0.0 / RADIAN2DEGREE_VALUE;
	}
	else if (x < -1e-6 && fabs(y) <= 1e-6)
	{
		return 180.0 / RADIAN2DEGREE_VALUE;
	}
	else
	{
		return atan2(y, x);
	}
}



/**
 * @brief range fmod
 *
 * @param x
 * @param min
 * @param max
 * @return float
 */
float RangeFmod(float x, float min, float max)
{
	float range = max - min;
	while(x < min)
	{
		x += range;
	}
	while(x > max)
	{
		x -= range;
	} 
	return x;
}



/**
 * @brief 简单滞回比较器
 *
 * @param state 上一时刻状态，0为低，1为高
 * @param input 比较输入
 * @param threshold_low 比较阈值，低
 * @param threshold_high 比较阈值，高
 * @return 状态，0为低，1为高，2为错误
 */
unsigned char SimpleHysteresis(unsigned char m_state, float input ,float threshold_low, float threshlod_high)
{

	if(0)
		return 2;
	else
	{
		if(m_state == 0)
		{
			if(input>threshlod_high)
				return 1;
			else
				return 0;
		}
		else if(m_state == 1)
		{
			if(input<threshold_low)
				return 0;
			else	
				return 1;
		}
		else
			return 2;	
	}
}



/**
 * @brief 一阶低通滤波器
 *
 * @param 
 * @param output_pre 上一时刻输出
 * @param input 输入
 * @param samp_freq 采样频率
 * @param cut_freq 截止频率
 * @return 
 */
double LPF(float output_pre, float input, float samp_freq, float cut_freq)
{
	double a = (double)1/(double)(1+samp_freq/(2*PI*cut_freq));
	// double output = (1-a) * (double)output_pre + a * (double)input;
	double output = (double)output_pre + a * (double)(input - output_pre);
	return output;
}

 /*-------------------------------------------------------------------
函数功能：判断浮点数是否相等
-------------------------------------------------------------------*/
bool Is_Float_Equal(float a, float b)
{
    return fabs(a-b)<1e-4f ? true:false;
}